{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix\n",
    "import sympy\n",
    "from robot import tr2rpy,rpy2tr\n",
    "from mdh_nao import Nao as mNao\n",
    "import cv2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q0 = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142]\n",
    "red = mNao(name=\"red\")\n",
    "red.LeftHand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "tr = red.LeftHand.fkine(q0)\n",
    "tq,err = red.LeftHand.ikine(tr)\n",
    "err # 逆运动学误差很小"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "tr = red.LeftHand.fkine(q0)\n",
    "tr[0,3] += 50 # x 方向移动 50mm\n",
    "tq,err = red.LeftHand.ikine(tr)\n",
    "err # 逆运动学误差变得很大"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "J0 = red.LeftHand.jacob0(q0)\n",
    "Jn = red.LeftHand.jacobn(q0)\n",
    "\n",
    "J0p = np.linalg.pinv(J0)\n",
    "Jnp = np.linalg.pinv(Jn)\n",
    "\n",
    "S,V,D = np.linalg.svd(J0)\n",
    "\n",
    "Matrix(V)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "奇异值共有5个,也就是说,该机械臂在世界坐标系中可达域为S的前五个向量组成的五维空间,\n",
    "\n",
    "另外,有一奇异值几乎为零,也就是说,目前的位姿接近一个奇异位姿"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 研究雅克比矩阵中的广义速度的定义"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "dq = np.matrix([0.0001]*5)\n",
    "dv = J0*dq.transpose()\n",
    "Matrix(dv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "tr = red.LeftHand.fkine(q0)\n",
    "tr2 = red.LeftHand.fkine(q0+dq)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def delta4tf(delta,tr):\n",
    "    x,y,z,dwx,dwy,dwz = array(delta).flatten()\n",
    "    p = np.matrix([x,y,z])\n",
    "    p.reshape((3,1))\n",
    "    tr2 = tr.copy()\n",
    "    tr2[0:3,3] += p.transpose()\n",
    "    w = np.matrix([dwx,dwy,dwz])\n",
    "    tr2[:3,:3] = cv2.Rodrigues(w)[0]*tr[:3,:3] \n",
    "    # 转动速度向量为在 0 坐标系下的绕定轴转动形式, \n",
    "    # 即绕 0 坐标系下 w=[x,y,z] 向量转动 theta=norm(w)\n",
    "    return tr2\n",
    "np.linalg.norm(delta4tf(dv,tr) - tr2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def delta4tf(delta,tr):\n",
    "    x,y,z,dwx,dwy,dwz = array(delta).flatten()\n",
    "    p = np.matrix([x,y,z])\n",
    "    p.reshape((3,1))\n",
    "    tr2 = tr.copy()\n",
    "    tr2[0:3,3] += p.transpose()\n",
    "    w = np.matrix([dwx,dwy,dwz])\n",
    "    tr2[:3,:3] = tr[:3,:3]*cv2.Rodrigues(w)[0]\n",
    "    # 转动速度向量为在末端坐标系下的绕定轴转动形式\n",
    "    return tr2\n",
    "np.linalg.norm(delta4tf(dv,tr) - tr2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def delta4tf(delta,tr):\n",
    "    x,y,z,dwx,dwy,dwz = array(delta).flatten()\n",
    "    p = np.matrix([x,y,z])\n",
    "    p.reshape((3,1))\n",
    "    tr2 = tr.copy()\n",
    "    tr2[0:3,3] += p.transpose()\n",
    "    w = tr2rpy(tr)\n",
    "    w[0,0] += dwz\n",
    "    w[0,1] += dwy\n",
    "    w[0,2] += dwx\n",
    "    tr2[:3,:3] = rpy2tr(w)[:3,:3] # 转动速度向量为在0坐标系下的rpy表示形式\n",
    "    return tr2\n",
    "np.linalg.norm(delta4tf(dv,tr) - tr2) # "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 双臂协同的雅克比讨论\n",
    "将双臂看做一个运动环,保持相对位姿不动即相对速度为0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def qlr2q(ql,qr):\n",
    "    q = hstack((qL,qR))\n",
    "    q = matrix(q).transpose()\n",
    "    return q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def q2qlr(q):\n",
    "    ql = q[0:5] # 提取出左臂的关节角\n",
    "    qr = q[5:10] # 提取出右臂的关节角\n",
    "    ql = np.array(ql).reshape((5,))\n",
    "    qr = np.array(qr).reshape((5,))\n",
    "    return ql,qr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "qL = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142]\n",
    "qR = [0.786008536816,-0.0466809943318,1.17838895321,0.835045695305,0.299782127142] # 起始位姿与 qL 对称"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def jacobian0lr(ql,qr=None):\n",
    "    if qr is None:\n",
    "        ql,qr = q2qlr(ql)\n",
    "    jql0 = red.LeftHand.jacob0(ql)\n",
    "    jqr0 = red.RightHand.jacob0(qr)\n",
    "    return hstack((jql0,-jqr0))\n",
    "jlr0 = jacobian0lr(qL,qR)\n",
    "assert (jlr0[:,0:5] == red.LeftHand.jacob0(qL)).all()\n",
    "assert (jlr0[:,5:10] == -red.RightHand.jacob0(qR)).all()\n",
    "q=qlr2q(qL,qR)\n",
    "assert (jlr0 == jacobian0lr(q)).all()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def q2c(q):\n",
    "    ql,qr = q2qlr(q)\n",
    "    tl = red.LeftHand.fkine(ql)\n",
    "    tr = red.RightHand.fkine(qr)\n",
    "    return np.linalg.inv(tl)*tr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q=qlr2q(qL,qR)\n",
    "jlr0 = jacobian0lr(q)\n",
    "U,V,S = np.linalg.svd(jlr0)\n",
    "St = S.transpose() # 零空间向量\n",
    "Matrix(V)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "for n in range(7,10):\n",
    "    print n,np.linalg.norm(q2c(q) - q2c(q+St[:,n]*0.001))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def z20(T): # 将接近 0 的用 0 替换\n",
    "    for i in range(T.shape[0]):\n",
    "        for j in range(T.shape[1]):\n",
    "            if abs(T[i,j]) < 1e-5:\n",
    "                T[i,j] = 0\n",
    "    return Matrix(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Q,R = np.linalg.qr(jlr0)\n",
    "np.testing.assert_almost_equal(jlr0,Q*R)\n",
    "z20(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "z20(jlr0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(hstack((R[:,4],R[:,9])))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
